#include "Matrix4x4.h"

Matrix4x4::Matrix4x4() :
	Matrix<4>()
{
}

Matrix4x4::Matrix4x4(const Matrix4x4& copy) :
	Matrix<4>(copy)
{

}

Matrix4x4::~Matrix4x4()
{
}

Matrix4x4& Matrix4x4::scale(const float sx, const float sy, const float sz)
{
	m[0][0] = sx;
	m[0][1] = 0;
	m[0][2] = 0;
	m[0][3] = 0;
	m[1][0] = 0;
	m[1][1] = sy;
	m[1][2] = 0;
	m[1][3] = 0;
	m[2][0] = 0;
	m[2][1] = 0;
	m[2][2] = sz;
	m[2][3] = 0;
	m[3][0] = 0;
	m[3][1] = 0;
	m[3][2] = 0;
	m[3][3] = 1;

	return *this;
}

Matrix4x4& Matrix4x4::rotateX(const float alpha)
{
	const float cosa = cos(alpha), sina = sin(alpha);

	m[0][0] = 1;
	m[0][1] = 0;
	m[0][2] = 0;
	m[0][3] = 0;
	m[1][0] = 0;
	m[1][1] = cosa;
	m[1][2] = -sina;
	m[1][3] = 0;
	m[2][0] = 0;
	m[2][1] = sina;
	m[2][2] = cosa;
	m[2][3] = 0;
	m[3][0] = 0;
	m[3][1] = 0;
	m[3][2] = 0;
	m[3][3] = 1;

	return *this;
}

Matrix4x4& Matrix4x4::rotateY(const float alpha)
{
	const float cosa = cos(alpha), sina = sin(alpha);

	m[0][0] = cosa;
	m[0][1] = 0;
	m[0][2] = sina;
	m[0][3] = 0;
	m[1][0] = 0;
	m[1][1] = 1;
	m[1][2] = 0;
	m[1][3] = 0;
	m[2][0] = -sina;
	m[2][1] = 0;
	m[2][2] = cosa;
	m[2][3] = 0;
	m[3][0] = 0;
	m[3][1] = 0;
	m[3][2] = 0;
	m[3][3] = 1;

	return *this;
}

Matrix4x4& Matrix4x4::rotateZ(const float alpha)
{
	const float cosa = cos(alpha), sina = sin(alpha);

	m[0][0] = cosa;
	m[0][1] = -sina;
	m[0][2] = 0;
	m[0][3] = 0;
	m[1][0] = sina;
	m[1][1] = cosa;
	m[1][2] = 0;
	m[1][3] = 0;
	m[2][0] = 0;
	m[2][1] = 0;
	m[2][2] = 1;
	m[2][3] = 0;
	m[3][0] = 0;
	m[3][1] = 0;
	m[3][2] = 0;
	m[3][3] = 1;

	return *this;
}

Matrix4x4& Matrix4x4::translate(const float tx, const float ty,
		const float tz)
{
	m[0][0] = 1;
	m[0][1] = 0;
	m[0][2] = 0;
	m[0][3] = tx;
	m[1][0] = 0;
	m[1][1] = 1;
	m[1][2] = 0;
	m[1][3] = ty;
	m[2][0] = 0;
	m[2][1] = 0;
	m[2][2] = 1;
	m[2][3] = tz;
	m[3][0] = 0;
	m[3][1] = 0;
	m[3][2] = 0;
	m[3][3] = 1;

	return *this;
}

void Matrix4x4::transformPoint(const SPoint& in, SPoint& out) const
{
	const float _x = in.x, _y = in.y, _z = in.z;//, _h = in.h;

	out.x = m[0][0]*_x + m[0][1]*_y + m[0][2]*_z;// + m[0][3]*_h;
	out.y = m[1][0]*_x + m[1][1]*_y + m[1][2]*_z;// + m[1][3]*_h;
	out.z = m[2][0]*_x + m[2][1]*_y + m[2][2]*_z;// + m[2][3]*_h;
//	out.h = m[3][0]*_x + m[3][1]*_y + m[3][2]*_z + m[3][3]*_h;
}

void Matrix4x4::test(void)
{
	Matrix4x4 m3;
	m3.scale(2., 3., 1.);

	Matrix4x4 m4;
	m4.translate(3., 2., 5.);

	m3.multLeft(m4);
	if ((m3.m[0][0] == 2.0) && (m3.m[0][1] == 0.0) && (m3.m[0][2] == 0.0)
			&& (m3.m[0][3] == 3.0) && (m3.m[1][0] == 0.0)
			&& (m3.m[1][1] == 3.0) && (m3.m[1][2] == 0.0)
			&& (m3.m[1][3] == 2.0) && (m3.m[2][0] == 0.0)
			&& (m3.m[2][1] == 0.0) && (m3.m[2][2] == 1.0)
			&& (m3.m[2][3] == 5.0) && (m3.m[3][0] == 0.0)
			&& (m3.m[3][1] == 0.0) && (m3.m[3][2] == 0.0)
			&& (m3.m[3][3] == 1.0))
	{
		std::cout << "CG1Matrix4x4.multLeft ok!" << std::endl;
	}
	else
	{
		std::cout << "CG1Matrix4x4.multLeft not ok!" << std::endl;
	}

	m3.multRight(m4);
	if ((m3.m[0][0] == 2.0) && (m3.m[0][1] == 0.0) && (m3.m[0][2] == 0.0)
			&& (m3.m[0][3] == 9.0) && (m3.m[1][0] == 0.0)
			&& (m3.m[1][1] == 3.0) && (m3.m[1][2] == 0.0)
			&& (m3.m[1][3] == 8.0) && (m3.m[2][0] == 0.0)
			&& (m3.m[2][1] == 0.0) && (m3.m[2][2] == 1.0) && (m3.m[2][3]
			== 10.0) && (m3.m[3][0] == 0.0) && (m3.m[3][1] == 0.0)
			&& (m3.m[3][2] == 0.0) && (m3.m[3][3] == 1.0))
	{
		std::cout << "CG1Matrix4x4.multRight ok!" << std::endl;
	}
	else
	{
		std::cout << "CG1Matrix4x4.multRight not ok!" << std::endl;
	}
}

//Matrix Matrix4x4::invert(Matrix4d m1) {
//   m00 = m12*m23*m31 - m13*m22*m31 + m13*m21*m32 - m11*m23*m32 - m12*m21*m33 + m11*m22*m33;
//   m01 = m03*m22*m31 - m02*m23*m31 - m03*m21*m32 + m01*m23*m32 + m02*m21*m33 - m01*m22*m33;
//   m02 = m02*m13*m31 - m03*m12*m31 + m03*m11*m32 - m01*m13*m32 - m02*m11*m33 + m01*m12*m33;
//   m03 = m03*m12*m21 - m02*m13*m21 - m03*m11*m22 + m01*m13*m22 + m02*m11*m23 - m01*m12*m23;
//   m10 = m13*m22*m30 - m12*m23*m30 - m13*m20*m32 + m10*m23*m32 + m12*m20*m33 - m10*m22*m33;
//   m11 = m02*m23*m30 - m03*m22*m30 + m03*m20*m32 - m00*m23*m32 - m02*m20*m33 + m00*m22*m33;
//   m12 = m03*m12*m30 - m02*m13*m30 - m03*m10*m32 + m00*m13*m32 + m02*m10*m33 - m00*m12*m33;
//   m13 = m02*m13*m20 - m03*m12*m20 + m03*m10*m22 - m00*m13*m22 - m02*m10*m23 + m00*m12*m23;
//   m20 = m11*m23*m30 - m13*m21*m30 + m13*m20*m31 - m10*m23*m31 - m11*m20*m33 + m10*m21*m33;
//   m21 = m03*m21*m30 - m01*m23*m30 - m03*m20*m31 + m00*m23*m31 + m01*m20*m33 - m00*m21*m33;
//   m22 = m01*m13*m30 - m03*m11*m30 + m03*m10*m31 - m00*m13*m31 - m01*m10*m33 + m00*m11*m33;
//   m23 = m03*m11*m20 - m01*m13*m20 - m03*m10*m21 + m00*m13*m21 + m01*m10*m23 - m00*m11*m23;
//   m30 = m12*m21*m30 - m11*m22*m30 - m12*m20*m31 + m10*m22*m31 + m11*m20*m32 - m10*m21*m32;
//   m31 = m01*m22*m30 - m02*m21*m30 + m02*m20*m31 - m00*m22*m31 - m01*m20*m32 + m00*m21*m32;
//   m32 = m02*m11*m30 - m01*m12*m30 - m02*m10*m31 + m00*m12*m31 + m01*m10*m32 - m00*m11*m32;
//   m33 = m01*m12*m20 - m02*m11*m20 + m02*m10*m21 - m00*m12*m21 - m01*m10*m22 + m00*m11*m22;
//   scale(1/m1.determinant());
//}
//
//
//
//   public double determinant() {
//   double value;
//   value =
//   m03 * m12 * m21 * m30-m02 * m13 * m21 * m30-m03 * m11 * m22 * m30+m01 * m13    * m22 * m30+
//   m02 * m11 * m23 * m30-m01 * m12 * m23 * m30-m03 * m12 * m20 * m31+m02 * m13    * m20 * m31+
//   m03 * m10 * m22 * m31-m00 * m13 * m22 * m31-m02 * m10 * m23 * m31+m00 * m12    * m23 * m31+
//   m03 * m11 * m20 * m32-m01 * m13 * m20 * m32-m03 * m10 * m21 * m32+m00 * m13    * m21 * m32+
//   m01 * m10 * m23 * m32-m00 * m11 * m23 * m32-m02 * m11 * m20 * m33+m01 * m12    * m20 * m33+
//   m02 * m10 * m21 * m33-m00 * m12 * m21 * m33-m01 * m10 * m22 * m33+m00 * m11    * m22 * m33;
//   return value;
//   }
//   
